/*
 * determineColor.cpp
 *
 *  Created on: Nov 26, 2012
 *      Author: robot
 */
#include <camera/camera.h>

using namespace cv;

/**
 *  Determine the color of a tag.
 */
int Camera::determineColor (Mat src)
{
	int R = 0, G = 0, B = 0;

	int startRows = (src.rows / 2) - 5;
	int startCols = (src.cols / 2) - 5;

	for (int i = startRows; i < startRows + 16; i += 5) {
		for (int j = startCols; j < startCols + 16; j += 5) {
			Point3_<uchar>* p = src.ptr<Point3_<uchar> >(i, j);
			B += p -> x;
			G += p -> y;
			R += p -> z;
		}
	}
	B = B / 9;
	R = R / 9;
	G = G / 9;

	if (R > G && R > B)
		if ((1.8 * R) > (G + B))
			return 2;

	if (G > R && G > B)
		if ((1.8 * G) > (R + B))
			return 3;

	if (B > R && B > G)
		if ((1.8 * B) > (R + G))
			return 4;

	if ((R + G + B) < 500)
		return 1;

	return -1;
}
